using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
//using Poly2Tri;


namespace GameStateManagementSample.Classes
{
    public class BezierHelper
    {
        static int steps = 10;
        public static void SetStep(int step)
        {
            steps = step;
        }
        public static bool BezierIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 lpt1, Vector2 lpt2)
        {

            int verticesCount = (3 - 2) * steps;
            Vector2[] vertices = new Vector2[verticesCount];

            Vector2? previous = null;

            float amount = 0f;
            for (int j = 0; j <= steps - 1; j++)
            {
                float x = (float)Math.Pow(1 - amount, 2) * point3.X + 2.0f * (1 - amount) * amount * point2.X + amount * amount * point1.X;
                float y = (float)Math.Pow(1 - amount, 2) * point3.Y + 2.0f * (1 - amount) * amount * point2.Y + amount * amount * point1.Y;
                Vector2 pnt = new Vector2(x, y);
                if (previous != null)
                {
                    var pt = GeometryHelper.LinesIntersect(previous.Value, pnt, lpt1, lpt2);
                    if (pt != null) return true;
                }
                previous = new Vector2(x, y);
                amount += 1f / (steps - 1);
            }
            return false;
        }

        public static float QuadraticBezierAngle(float u, Vector2 a, Vector2 b, Vector2 c)
        {
            float uc = 1 - u;
            float dx = (uc * b.X + u * c.X) - (uc * a.X + u * b.X);
            float dy = (uc * b.Y + u * c.Y) - (uc * a.Y + u * b.Y);
            
            return (float)Math.Atan2(dy, dx);
        }
        float CubicBezierTangentAt(float t, float a, float b, float c)
        {
            // note that abcd are aka x0 x1 x2 x3

            /*  the four coefficients ..
                A = x3 - 3 * x2 + 3 * x1 - x0
                B = 3 * x2 - 6 * x1 + 3 * x0
                C = 3 * x1 - 3 * x0
                D = x0

                and then...
                Vx = 3At2 + 2Bt + C         */

            // first calcuate what are usually know as the coeffients,
            // they are trivial based on the four control points:

            float C1 = ((2.0f * c) + (2.0f * b) - a);
            float C2 = ((2.0f * c) - (4.0f * b) + (2.0f * a));
            float C3 = ((3.0f * b) - (3.0f * a));
            float C4 = (a);  // (not needed for this calculation)

            // finally it is easy to calculate the slope element, using those coefficients:

            return ((3.0f * C1 * t * t) + (2.0f * C2 * t) + C3);

            // note that this routine works for both the x and y side;
            // simply run this routine twice, once for x once for y
            // note that there are sometimes said to be 8 (not 4) coefficients,
            // these are simply the four for x and four for y, calculated as above in each case.
        }

        float CubicBezierTangentAt(float t, float a, float b, float c, float d)
        {
            // note that abcd are aka x0 x1 x2 x3

            /*  the four coefficients ..
                A = x3 - 3 * x2 + 3 * x1 - x0
                B = 3 * x2 - 6 * x1 + 3 * x0
                C = 3 * x1 - 3 * x0
                D = x0

                and then...
                Vx = 3At2 + 2Bt + C         */

            // first calcuate what are usually know as the coeffients,
            // they are trivial based on the four control points:

            float C1 = (d - (3.0f * c) + (3.0f * b) - a);
            float C2 = ((3.0f * c) - (6.0f * b) + (3.0f * a));
            float C3 = ((3.0f * b) - (3.0f * a));
            float C4 = (a);  // (not needed for this calculation)

            // finally it is easy to calculate the slope element, using those coefficients:

            return ((3.0f * C1 * t * t) + (2.0f * C2 * t) + C3);

            // note that this routine works for both the x and y side;
            // simply run this routine twice, once for x once for y
            // note that there are sometimes said to be 8 (not 4) coefficients,
            // these are simply the four for x and four for y, calculated as above in each case.
        }


        public static float GetCubicBezierPointAt(float t, float start, float control_1, float control_2, float end)
        {
            /* Formula from Wikipedia article on Bezier curves. */
            return start * (1f - t) * (1f - t) * (1f - t)
                   + 3f * control_1 * (1f - t) * (1f - t) * t
                   + 3f * control_2 * (1f - t) * t * t
                   + end * t * t * t;
        }
        public static Vector2 GetCubicBezierPointAt(float t, Vector2 start, Vector2 control_1, Vector2 control_2, Vector2 end)
        {
            /* Formula from Wikipedia article on Bezier curves. */
            float X = start.X * (1f - t) * (1f - t) * (1f - t)
                    + 3f * control_1.X * (1f - t) * (1f - t) * t
                    + 3f * control_2.X * (1f - t) * t * t
                    + end.X * t * t * t;
            float Y = start.Y * (1f - t) * (1f - t) * (1f - t)
                   + 3f * control_1.Y * (1f - t) * (1f - t) * t
                   + 3f * control_2.Y * (1f - t) * t * t
                   + end.Y * t * t * t;
            return new Vector2(X, Y);
        }

        public static float GetQuadraticBezierPointAt(float t, float start, float control_1, float end)
        {
            /* Formula from Wikipedia article on Bezier curves. */
            return start * (1f - t) * (1f - t)
                   + 2f * control_1 * (1f - t) * t
                   + end * t * t;
        }

        public static Vector2 GetQuadraticBezierPointAt(float t, BezierSegment bs)
        {
            /* Formula from Wikipedia article on Bezier curves. */
            float X = bs.p1.X * (1f - t) * (1f - t)
                   + 2f * bs.p2.X * (1f - t) * t
                   + bs.p3.X * t * t;
            float Y = bs.p1.Y * (1f - t) * (1f - t)
                   + 2f * bs.p2.Y * (1f - t) * t
                   + bs.p3.Y * t * t;
            return new Vector2(X, Y);
        }

        public static Vector2 GetQuadraticBezierPointAt(float t, Vector2 start, Vector2 control_1, Vector2 end)
        {
            /* Formula from Wikipedia article on Bezier curves. */
            float X = start.X * (1f - t) * (1f - t)
                   + 2f * control_1.X * (1f - t) * t
                   + end.X * t * t;
            float Y = start.Y * (1f - t) * (1f - t)
                   + 2f * control_1.Y * (1f - t) * t
                   + end.Y * t * t;
            return new Vector2(X, Y);
        }
        //public static PolygonPoint GetQuadraticBezierPointAt(float t, Vector2 start, Vector2 control_1, Vector2 end)
        //{
        //    /* Formula from Wikipedia article on Bezier curves. */
        //    float X = start.X * (1f - t) * (1f - t)
        //           + 2f * control_1.X * (1f - t) * t
        //           + end.X * t * t;
        //    float Y = start.Y * (1f - t) * (1f - t)
        //           + 2f * control_1.Y * (1f - t) * t
        //           + end.Y * t * t;
        //    return new PolygonPoint(X, Y);
        //}
        public static float GetCubicBezierLength(Vector2 A, Vector2 B, Vector2 C, Vector2 D)
        {
            float t;
            int i;

            Vector2 dot = new Vector2();
            Vector2 previous_dot = new Vector2(0, 0);
            float length = 0f;

            for (i = 0; i <= steps; i++)
            {
                t = (float)i / (float)steps;
                dot.X = GetCubicBezierPointAt(t, A.X, B.X,
                               C.X, D.X);
                dot.Y = GetCubicBezierPointAt(t, A.Y, B.Y,
                               C.Y, D.Y);
                if (i > 0)
                {
                    //float x_diff = dot.X - previous_dot.X;
                    //float y_diff = dot.Y - previous_dot.Y;
                    //length += (float)Math.Sqrt(x_diff * x_diff + y_diff * y_diff);
                    length += Vector2.Distance(previous_dot, dot);
                }
                previous_dot = dot;
            }
            return length;
        }
        public static float GetQuadraticBezierLength(Vector2 A, Vector2 B, Vector2 C)
        {
            float t;
            int i;

            Vector2 dot = new Vector2();
            Vector2 previous_dot = new Vector2(0, 0);
            float length = 0f;

            for (i = 0; i <= steps; i++)
            {
                t = (float)i / (float)steps;
                dot.X = GetQuadraticBezierPointAt(t, A.X, B.X,
                               C.X);
                dot.Y = GetQuadraticBezierPointAt(t, A.Y, B.Y,
                               C.Y);
                if (i > 0)
                {
                    //float x_diff = dot.X - previous_dot.X;
                    //float y_diff = dot.Y - previous_dot.Y;
                    //length += (float)Math.Sqrt(x_diff * x_diff + y_diff * y_diff);
                    length += Vector2.Distance(previous_dot, dot);

                }
                previous_dot = dot;
            }
            return length;
        }
    }
}
